import pickle
import numpy as np
import Qlearning
import Plotmaze

length = 20
width = 20
height = 10
startPoint = (0, 0, 0)
endPoint = (length - 1, width - 1, int(0.5 * height) - 1)  # 基础信息，与train保持相同
with open('../pkl/Nodedict2700000.pkl', 'rb') as f:  # 模型加载
    pointdata = pickle.load(f)
with open('../pkl/obstacle.pkl', 'rb') as f:
    obstacledata = pickle.load(f)  # 加载策略与地图

pointdata[endPoint].Q = np.zeros(26) + 1000
for i in pointdata:  # 坐标不在地图中时，删除防止报错
    if i[0] >= length or i[0] < 0:
        pointdata.pop(i)
    elif i[1] >= width or i[1] < 0:
        pointdata.pop(i)
    elif i[2] >= height or i[2] < 0:
        pointdata.pop(i)

    for j in range(26):  # 以防训练不到位，产生未训练到的行动，置为大负值
        if pointdata[i].Q[j] == 0:
            pointdata[i].Q[j] = -300
    print(type(pointdata[i].Q), i, pointdata[i].Q)

    # print(i)
currnode = pointdata[startPoint]
printlist = []
printlist.append(startPoint)

while (currnode.point.x, currnode.point.y, currnode.point.z) != (
        pointdata[endPoint].point.x, pointdata[endPoint].point.y, pointdata[endPoint].point.z):
    # 开始按照策略进行路径生成
    flag = 0
    maxQ = np.max(currnode.Q)
    maxact = np.where(currnode.Q == maxQ)[0][0]
    nextpointxyz = Qlearning.nextPoint(currnode.point, maxact)
    nextpoint = (nextpointxyz.x, nextpointxyz.y, nextpointxyz.z)
    nextnode = pointdata[nextpoint]
    for i in obstacledata:
        if i[0] <= nextpointxyz.x <= i[0] + 1 and i[1] <= nextpointxyz.y <= i[1] + 1 and \
                i[2] >= nextpointxyz.z:
            # 触碰障碍物
            flag = 1
            print(i, "  ", nextpoint)
            break

    if flag == 0:  # 未触碰障碍物时，绘制点
        printlist.append(nextpoint)
        print("flag==0: ", nextpoint)
        currnode = nextnode
        Plotmaze.plotpoint(obstacledata, startPoint, endPoint, printlist)
    else:
        currnode.Q[maxact] = currnode.Q[maxact] - 1000  # 触碰障碍物时，更改Q值，重新选点

Plotmaze.pathpoint(obstacledata, startPoint, endPoint, printlist)  # 画图
print(len(printlist))
